import sensor, image, time, pyb, lcd, math
from pyb import LED, UART
usart3 = pyb.UART(3, 115200, bits=8, parity=None, stop=1, timeout_char=1000)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.LCD)
sensor.skip_frames(time=2000)
sensor.set_auto_whitebal(False)
sensor.set_auto_gain(False)
lcd.init()

Mode=1

LED(3).on()
clock = time.clock()

juxin_list = []
red=(20, 97, 14, 124, -128, 127)#(68, 100, -14, 35, 10, 40)
red2=(20, 94, 1, 46, 19, 37)
def UART_Out1(Dian):
    data = bytearray([0xb3, 0xb3, int(Dian[0]), int(Dian[1]), int(Dian[2]),
                                  int(Dian[3]), int(Dian[4]), int(Dian[5]),
                                  int(Dian[6]), int(Dian[7]), int(Dian[8]),0x5b])
    usart3.write(data)
def UART_Out2(redxy):
    data = bytearray([0xb3, 0xb3, int(redxy[0]), int(redxy[1]), 0x5b])
    usart3.write(data)
while True:
    clock.tick()

    img = sensor.snapshot().lens_corr(1.2)
    img.draw_rectangle((5,10,110,140))
    corners = []
    if Mode==0:
        r_blobs = img.find_blobs([red],x_stride =2,y_stride =2,area_threshold =2,pixels_threshold =10)
        sensor.set_auto_exposure(False,8000)
        if r_blobs:
            img.draw_rectangle(r_blobs[0].rect(),color =(255,0,0))
            redxy=[r_blobs[0].cx(), r_blobs[0].cy()]
            UART_Out2(redxy)
            print(r_blobs)
        else:
            red2_blobs = img.find_blobs([red2],x_stride =2,y_stride =2,area_threshold =2,pixels_threshold =2)
            if red2_blobs:
                if red2_blobs[0].pixels()<100:
                    img.draw_rectangle(red2_blobs[0].rect(),color =(0,255,0))
                    redxy=[red2_blobs[0].cx(), red2_blobs[0].cy()]
                    UART_Out2(redxy)
    else:
        duanbian=0
        Dian=[]
        rects = img.find_rects(roi =(5,10,110,140),threshold=12000)
        if rects:
            img.draw_rectangle(rects[0].rect(), color=(255, 0, 0))
            #print(rects[0])
            kuang = rects[0].corners()
            changdu1=math.sqrt( ((kuang[0][0]-kuang[1][0])*(kuang[0][0]-kuang[1][0]))
                                    + (kuang[0][1]-kuang[1][1])*(kuang[0][1]-kuang[1][1]) )
            changdu2=math.sqrt( ((kuang[0][0]-kuang[3][0])*(kuang[0][0]-kuang[3][0]))
                                    + (kuang[0][1]-kuang[3][1])*(kuang[0][1]-kuang[3][1]) )
            if changdu1>changdu2:
                duanbian=changdu2
            else:
                duanbian=changdu1
            if duanbian>86:
                Dian.append(0x01)
            else:
                Dian.append(0x02)
            for p in kuang:
                img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
                Dian.append(p[0])
                Dian.append(p[1])
            UART_Out1(Dian)
            print(Dian)
    if(usart3.any()>0):
        a=usart3.readchar()
        Mode=0
        LED(3).off()
        LED(1).on()
    img.draw_string(0,0,"fps:%d"%(int(clock.fps())),color = (255, 255, 0), scale = 2)
    lcd.display(img)
